package behavior;

import arena.Node;
import java.util.LinkedList;
import java.util.List;

/**
 * AStarBehavior implements a best-first graph searching algorithm
 * which is used to send a robot to a specific node in the most
 * efficient way possible.
 *
 * @author      Gabriel Loewen
 * @author      James Weston
 * @author      Jackie O'Quinn
 */
public class AStarBehavior extends Behavior {

    /**
     * Linked list that holds each node in the generated path
     */
    private LinkedList<Node> path = new LinkedList<Node>();         // The path to the goal
    /**
     * A reference to the goal node
     */
    private Node goalNode;
    private double progress = 0;

    /**
     * Default constructor
     */
    public AStarBehavior() {
        super("A*");
    }

    /**
     * Returns a boolean value for whether or not a move is waiting to be
     * executed.
     */
    public boolean hasMove() {

        //avoids NullPointerExceptions
        if(path.peek() == null)
            return false;

        Node node = path.peek();

        //if the robot has reached the goal, stop (fail safe)
        if(robot.getNode().getLocation().equals(goalNode.getLocation())){
            path.clear();
            return false;
        }

        //if there is an obsticle, recalculate path
        if (node.getColor() == Node.DELETED || !Node.getAdjNodes(node).contains(robot.getNode())) {
            path.clear();
            getPath(goalNode.toString(), false);
            //if goal is blocked
            if (path.isEmpty()) {
                return false;
            }
        }
        return !path.isEmpty() && !(node.isOccupied);
    }

    /**
     * Returns an integer array containing the data needed for the
     * next move in the queue.
     *
     * @see Util
     */
    public int[] nextMove() {

        if (path.isEmpty()) {
            return null;
        }
        
        Node node = path.pop();
        int dx = node.getLocation().x - robot.getLocation().x;
        int dy = node.getLocation().y - robot.getLocation().y;

        char dir;
        if (dx == 0) {
            if (dy > 0) {
                dir = 'N';
            } else {
                dir = 'S';
            }
        } else {
            if (dx > 0) {
                dir = 'E';
            } else {
                dir = 'W';
            }
        }

       return Util.getMoveData(robot.heading, robot.getLocation().x, robot.getLocation().y, dir);
    }

    /**
     * Calculates the path through the nodes using the a-star algorithm
     * and populates a queue with the moves necessary to complete the
     * path
     *
     * @param   goal    The destination in (x,y) format.  i.e. "4,3" would
     * specify a goal of the coordinate (4,3).
     * @param   cancel  Boolean variable that cancels the algorithm if the user
     * presses the cancel button.
     */
    public void getPath(final String goal, boolean cancel) {
        if (!isValidInput(goal) || robot.getArena().nodeMap.get(goal).isOccupied || cancel) {
            return;
        }

        Node startNode = robot.getArena().nodeMap.get(robot.getLocation().toString());
        goalNode = robot.getArena().nodeMap.get(goal);

        PriorityList openList = new PriorityList();
        LinkedList closedList = new LinkedList();

        startNode.costFromStart = 0;
        startNode.estimatedCostToGoal = Node.getEstimatedCost(startNode, goalNode);
        startNode.pathParent = null;
        openList.add(startNode);
        int iterations = 0;
        while (!openList.isEmpty() && !cancel) {
            iterations++;
            Node node = (Node) openList.removeFirst();
            progress = ((double) iterations / (double) (robot.getArena().height * robot.getArena().width)) * 100.0;
            if (node == goalNode) {
                progress = 100;
                // construct the path from start to goal
                constructPath(goalNode);
                break;
            }

            List<Node> neighbors = Node.getAdjNodes(node);
            for (Node neighborNode : neighbors) {
                if (neighborNode.getColor() == Node.DELETED) {
                    closedList.add(neighborNode);
                    openList.remove(neighborNode);
                    continue;
                }
                boolean isOpen = openList.contains(neighborNode);
                boolean isClosed =
                        closedList.contains(neighborNode);
                float costFromStart = node.costFromStart +
                        node.getCost(neighborNode);

                // check if the neighbor node has not been
                // traversed or if a shorter path to this
                // neighbor node is found.
                if (((!isOpen && !isClosed) ||
                        costFromStart <= neighborNode.costFromStart) &&
                        nodeExists(neighborNode.toString())) {
                    neighborNode.pathParent = node;
                    neighborNode.costFromStart = costFromStart;
                    neighborNode.estimatedCostToGoal =
                            Node.getEstimatedCost(neighborNode, goalNode);
                    if (isClosed) {
                        closedList.remove(neighborNode);
                    }
                    if (!isOpen) {
                        openList.add(neighborNode);
                    }
                }
            }
            closedList.add(node);
        }

    // no path found

    //messageThread.hide();
    }

    /**
     * Returns true if the next move is valid.  This is used as a safety
     * precaution in case the algorithm produced an invalid move.
     *
     * @param   input   The next move in (x,y) format.  i.e. "4,3" specifies
     * the next move as (4,3)
     * @return  True if the input is valid.
     */
    public boolean isValidInput(String input) {
        return robot.getArena().nodeMap.containsKey(input);
    }

    /**
     * Returns true if it is ok to continue moving.  If its not ok to continue
     * then the method breaks and should cease all robot movement.
     */
    public boolean isValidMove() {
        if (path.size() == 0) {
            return false;
        }
        Node node = path.peek();

        if (!node.isOccupied) {
            return true;
        } else {
            if (node.equals(goalNode)) {
                path.clear();
            }
            return false;
        }
    }

    /**
     * Constructs a path from the start node to the goal node.  This path
     * is calculated using the getPath method.
     *
     * @param   node    The goal node object
     */
    protected synchronized void constructPath(Node node) {
        while (node.pathParent != null) {
            path.addFirst(node);
            node = node.pathParent;
        }

    }

    /**
     * Prints the complete path to the console.  Used for debugging purposes
     * only.
     */
    protected void printPath() {
        System.out.print("Printing path: ");
        for (Node node: path) {
            System.out.print(node + " : ");
        }

        System.out.println();
    }

    public double getProgress() {
        return progress;
    }

    public void resetProgress() {
        progress = 0;
    }

    /**
     * A custom priorized list extended from the LinkedList class.
     *
     * @author Gabriel Loewen
     * @see java.util.LinkedList
     */
    public static class PriorityList extends LinkedList {

        /**
         * Adds an object to the list with a priority
         *
         * @param   object  Object to add to the list
         */
        public void add(Comparable object) {
            for (int i = 0; i < size(); i++) {
                if (object.compareTo(get(i)) <= 0) {
                    add(i, object);
                    return;
                }
            }
            addLast(object);
        }
    }
}
